// Test_Controller.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"

#include "RobotPlant.RobotProxy\RobotPlant.RobotProxy.h"

using namespace RobotPlant::RobotProxy;

char *deviceTypeName[13] = {
	"Unknow",
	"DeviceDictionary",
	"AnalogSensorDevice",
	"ArmDevice",
	"BatteryDevice",
	"CameraDevice",
	"ContactSensorDevice",
	"DiffDriveDevice",
	"EncoderDevice",
	"MotorDevice",
	"SickLRFDevice",
	"SonarDevice",
	"StreamDevice"};

void Test_AnalogSensorDevice()
{
	printf("Test AnalogSensorDevice start...\n");

	RobotManager x;
	RobotInstance robot("http://localhost:8080/RobDeviceInterface/");
	AnalogSensorDevice device(&robot);

	int index = device.GetDeviceIndex();
	printf("Device Index is %d.\n", index);

	DeviceTypeEnum type = device.GetDeviceType();
	printf("Device Type is %s.\n", deviceTypeName[(int)type]);

	int hwID = device.HardwareID();
	printf("Hardware ID is %d.\n", hwID);

	string name = device.Name();
	printf("Device name is %s.\n", name);

	std::vector<double> pose = device.Pose();
	printf("Device pose is %f  %f  %f %f  %f  %f %f.\n", pose[0], pose[1], pose[2], pose[3], pose[4], pose[5], pose[6]);

	double normalizedMeasurement = device.GetNormalizedMeasurement();
	printf("Normalized Measurement is %f.\n", normalizedMeasurement);

	double rawMeasurement = device.GetRawMeasurement();
	printf("Raw Measurement is %f.\n", rawMeasurement);

	double rmRange = device.GetRawMeasurementRange();
	printf("Raw Measurement Range is %f.\n", rmRange);

	time_t myTime = device.GetLastTimeStamp();
	printf("Last time stamp is %s.\n", ctime(&myTime));


	printf("Test AnalogSensorDevice end.\n\n\n");

}


void Test_CameraDevice()
{
	printf("Test CameraDevice start...\n");

	RobotManager x;
	RobotInstance robot("http://localhost:8080/RobDeviceInterface/");
	CameraDevice device(&robot);

	int index = device.GetDeviceIndex();
	printf("Device Index is %d.\n", index);

	DeviceTypeEnum type = device.GetDeviceType();
	printf("Device Type is %s.\n", deviceTypeName[(int)type]);

	int hwID = device.HardwareID();
	printf("Hardware ID is %d.\n", hwID);

	string name = device.Name();
	printf("Device name is %s.\n", name);

	std::vector<double> pose = device.Pose();
	printf("Device pose is %f  %f  %f %f  %f  %f %f.\n", pose[0], pose[1], pose[2], pose[3], pose[4], pose[5], pose[6]);

	std::vector<int> imageSize = device.GetImageSize();
	printf("Image size is %d  %d.\n", imageSize[0], imageSize[1]);

	double viewAngle = device.GetViewAngle();
	printf("View Angle is %f.\n", viewAngle);

	double quality = device.GetQuality();
	printf("Quality is %f.\n", quality);

	BMP lastFrame = device.GetLastFrame();
	FILE * bmpFile = fopen("lastframe.bmp","wb");
	fwrite (lastFrame.Data, 1, lastFrame.Size, bmpFile );
	fclose (bmpFile);
	printf("View Angle is %f.\n", viewAngle);

	time_t myTime = device.GetLastTimeStamp();
	printf("Last frame is lastframe.bmp.\n", ctime(&myTime));

	printf("Test CameraDevice end.\n\n\n");
}

int _tmain(int argc, _TCHAR* argv[])
{
	Test_AnalogSensorDevice();
	Test_CameraDevice();

	system("pause");
	return 0;
}

